//
// Created by Shin on 2023/11/9.
//

#include "Sensor.h"

imu_t ros_imu;

void sensor_init(void)
{
    ros_imu.is_alive = 0;

    ros_imu.pitch = .0f;
    ros_imu.roll = .0f;
    ros_imu.yaw = .0f;

    ros_imu.gyro[0] = 0;
    ros_imu.gyro[1] = 0;
    ros_imu.gyro[2] = 0;

    ros_imu.accel[0] = 0;
    ros_imu.accel[1] = 0;
    ros_imu.accel[2] = 0;

    ros_imu.quat[0] = 0;
    ros_imu.quat[1] = 0;
    ros_imu.quat[2] = 0;
    ros_imu.quat[3] = 0;

    ros_imu.is_latest[0] = 0;
    ros_imu.is_latest[1] = 0;
    ros_imu.is_latest[2] = 0;
    ros_imu.is_latest[3] = 0;

#if USE_WIT
    WIT_reset();
#endif
    ros_imu.is_alive = 1;
}

uint8_t imu_data_sum_crc(uint8_t *data, int data_num)
{
#if USE_MPU6050
    //TODO
#elif USE_WIT
    return WIT_data_sum_crc(data, data_num);
#endif
}

void imu_data_update(uint8_t *data_source)
{
#if USE_WIT
    WIT_data_update(data_source);
#endif
//        print_UARTx(&huart1, "(%d %d %d)(%d %d %d)(%d %d %d)(%d %d %d %d)\r\n",
//                    (int) ros_imu.pitch, (int) ros_imu.roll, (int) ros_imu.yaw,
//                    (int) ros_imu.gyro[0], (int) ros_imu.gyro[1], (int) ros_imu.gyro[2],
//                    (int) ros_imu.accel[0], (int) ros_imu.accel[1], (int) ros_imu.accel[2],
//                    (int) (ros_imu.quat[0] * 100), (int) (ros_imu.quat[1] * 100),
//                    (int) (ros_imu.quat[2] * 100), (int) (ros_imu.quat[3] * 100));

}

int imu_is_ready(void)
{
    return ros_imu.is_alive;
}

uint8_t imu_get_data(float *pitch,
                     float *roll,
                     float *yaw,
                     float gyro[3],
                     float accel[3],
                     float quat[4])
{
    if( ros_imu.is_latest[0] +
        ros_imu.is_latest[1] +
        ros_imu.is_latest[2] +
        ros_imu.is_latest[3] != 4){
        return 0;
    }
    ros_imu.is_latest[0] = 0;
    ros_imu.is_latest[1] = 0;
    ros_imu.is_latest[2] = 0;
    ros_imu.is_latest[3] = 0;

    *pitch = ros_imu.pitch;
    *roll = ros_imu.roll;
    *yaw = ros_imu.yaw;

    gyro[0] = ros_imu.gyro[0];
    gyro[1] = ros_imu.gyro[1];
    gyro[2] = ros_imu.gyro[2];

    accel[0] = ros_imu.accel[0];
    accel[1] = ros_imu.accel[1];
    accel[2] = ros_imu.accel[2];

    quat[0] = ros_imu.quat[0];
    quat[1] = ros_imu.quat[1];
    quat[2] = ros_imu.quat[2];
    quat[3] = ros_imu.quat[3];
    return 1;
}

